#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <QtDebug>
#include <Qtimer>
#include <QPainter>
#include <stdio.h>
#include <windows.h>
#include <assert.h>
/* Communication code */

// macro to make debug output a bit nicer.
#define _FILE_LINE_ __FILE__<<':'<<__LINE__

HANDLE hCom = NULL;
struct comPacketHeader {
	uint16_t type;
	uint16_t size;
};
struct comPacket {
	struct comPacketHeader header;
	char* data;
};

enum eTYPES {
    TYPE_WALLS = 0x01,
    TYPE_TWO,
    TYPE_IMAGE,
    TYPE_START,
    TYPE_STOP,
    TYPE_MOTOR_SET,
    TYPE_FRONTLED_SET,
	TYPE_MAP,
    TYPE_OBJECT_FOUND,
    TYPE_RESET_COM,
    TYPE_MAX
};

//Objects
enum eOBJECTS{
    OBJ_UNKNOWN = 0,
    OBJ_SQUARE,
    OBJ_TRIANGLE,
    OBJ_BAR,
    OBJ_SIZE //the size of the enum eOBJECTS
};

//Colors
enum eCOLOR{
    COLOR_GREEN = 0,
    COLOR_RED,
    COLOR_BLUE,
    COLOR_SIZE //the size of the enum eCOLOR
};

void drawObjectOnPixmap(QPixmap &px, QPoint loc, enum eOBJECTS obj ,enum eCOLOR color);

struct comPacket comP;
unsigned int comReadPos;
unsigned int comPortList[32];

char* getComPortName(int port) {
	static char str[256];
	snprintf(str,255,"\\\\.\\COM%d",port);
	str[255] = '\0';
	return str;
}

// Returns 0 on success, otherwise error
int openCom(int port) {
	DCB dcb;
	COMMTIMEOUTS timeouts;
	HANDLE h = CreateFileA(
				getComPortName(port),
				(GENERIC_READ | GENERIC_WRITE),
				0,
				NULL,
				OPEN_EXISTING,
				FILE_FLAG_OVERLAPPED,
				NULL
	);

	if(h == INVALID_HANDLE_VALUE) return 1;
	if (!GetCommState(h,&dcb)) {
		CloseHandle(h);
		return 2;
	}

	dcb.BaudRate = 115200;
	dcb.fParity = FALSE;
	dcb.Parity = NOPARITY;
	dcb.StopBits = ONESTOPBIT;
	dcb.ByteSize = 8;
	dcb.fOutxCtsFlow = FALSE;
	dcb.fOutxDsrFlow = FALSE;
	dcb.fDtrControl = DTR_CONTROL_DISABLE;
	dcb.fRtsControl = RTS_CONTROL_DISABLE;

	if (!BuildCommDCBA("115200,n,8,1", &dcb)) {
		CloseHandle(h);
		return 3;
	}

	if (!SetCommState(h, &dcb)) {
		CloseHandle(h);
		return 4;
	}

    timeouts.ReadIntervalTimeout = 1;
    timeouts.ReadTotalTimeoutMultiplier = 1;
    timeouts.ReadTotalTimeoutConstant = 1;
    timeouts.WriteTotalTimeoutMultiplier = 1;
    timeouts.WriteTotalTimeoutConstant = 1;
	if (!SetCommTimeouts(h, &timeouts)){
        CloseHandle(h);
		return 5;
	}

	hCom = h;
	comP.data = NULL;
	comReadPos = 0;
	return 0;
}

void closeCom(void) {
	if(!hCom) return;
	CloseHandle(hCom); // Whilst CloseHandle can return erors, we ignore it's cries for help. 8)
	hCom = NULL;
}

int readCom(void* buffer, int len) { // returns amount of bytes read on success, 0 otherwise
	OVERLAPPED osRead = {0};
	DWORD bytesRead;
	osRead.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
	if(osRead.hEvent == NULL) return 0;
	ReadFile(hCom,buffer,len,&bytesRead,&osRead);
	DWORD res = WaitForSingleObject(osRead.hEvent, 100);
	if(res == WAIT_OBJECT_0) { // obj read
		if(!GetOverlappedResult(hCom, &osRead, &bytesRead, FALSE)) {
			// err
			qDebug() << "GetOverlappedResult failed." << _FILE_LINE_;
			return 0;
		} else {
			if(bytesRead != 0) qDebug() << "Read " << bytesRead << "/" << len;
			return bytesRead;
		}
	} else if(res == WAIT_TIMEOUT) {
        //qDebug() << "Error timeout read." << _FILE_LINE_;
		return 0;
	} else {
		qDebug() << "Could not wait for object." << _FILE_LINE_;
	}
	return bytesRead;
}
int writeCom(void* buffer, int len) { // returns amount of bytes written on success, 0 otherwise
	OVERLAPPED osWrite = {0};
	DWORD bytesWritten;
	if(WriteFile(hCom,buffer,len,&bytesWritten,&osWrite)) return 0;
	return bytesWritten;
}

// Returns 1 if a packet was succesfully read and finished reading. (See comP for result)
// Returns 0 if nothing happened or a packet was not yet completely read.
// Returns -1 on errors.
int readPacket() { // returns 1 if a packet succesfully was read, 2 if a packet was partially read, 0 otherwise.
	if(comReadPos < sizeof(comPacketHeader)) {
		int bytesRead = readCom(((char*)(&(comP.header))) + comReadPos,
								sizeof(comPacketHeader) - comReadPos);
		if(bytesRead == 0) {
			DWORD err = GetLastError();
			if(err != ERROR_IO_PENDING && err != ERROR_SUCCESS) {
				qDebug() << "Error reading file: " << err << "." << _FILE_LINE_;
				return -1;
			}
		} else {
			comReadPos += bytesRead;
			if(comReadPos == sizeof(comPacketHeader)) {
				if(comP.data != NULL) free(comP.data);
				comP.data = (char*)malloc(comP.header.size);
				assert(comP.data != NULL);
			}
		}
	}
	if(comReadPos >= sizeof(comPacketHeader)) {
		if(comReadPos == comP.header.size + sizeof(comPacketHeader)) {
			comReadPos = 0;
			return 1;
		}
		int bytesRead = readCom(comP.data + comReadPos - sizeof(comPacketHeader),
								comP.header.size - comReadPos + sizeof(comPacketHeader));
		if(bytesRead == 0) {
			DWORD err = GetLastError();
			if(err != ERROR_IO_PENDING && err != ERROR_SUCCESS) {
				qDebug() << "Error reading file:" << err << "." << _FILE_LINE_;
				return -1;
			}
		} else {
			comReadPos += bytesRead;
			if(comReadPos == comP.header.size + sizeof(comPacketHeader)) {
				comReadPos = 0;
				return 1;
			}
		}
	}
	return 0;
}

// Returns 0 normally, -1 on errors.
int writePacket(uint16_t type, uint16_t len, void* data) {
	unsigned int bytesWritten;

	struct comPacketHeader p;
	p.type = type;
	p.size = len;

	bytesWritten = writeCom(&p,sizeof(comPacketHeader));
	if(bytesWritten == 0 || bytesWritten < sizeof(comPacketHeader)) {
		DWORD err = GetLastError();
		if(err != ERROR_IO_PENDING) {
			//
			return -1;
		}
	}

	bytesWritten = writeCom(data,len);
	if(bytesWritten == 0 || bytesWritten < len) {
		DWORD err = GetLastError();
		if(err != ERROR_IO_PENDING) {
			//
			return -1;
		}
	}
	return 0;
}

// Checks COM ports (from) to (to) whether they're valid. The first 32 valid ones are saved in comPortList.
// Returns the amount of valid COM ports found.
int fillComPortList(int from, int to) {
	int count = 0;
	for(int i = 0; i<32; i++) {
		comPortList[i] = -1;
	}
	for(int i = from; i<to && count<32; i++) {
		int status = openCom(i);
		if(status == 0) {
			closeCom();
			comPortList[count] = i;
			count++;
		}
	}
	return count;
}

/* main window code */

void MainWindow::update(void) {
    comTimer->stop();

    if(hCom != NULL) {
		int ret = readPacket();
        if(ret == 1) {
			qDebug() << "Packet read, size = " << comP.header.size << ", type = " << comP.header.type << ".";
            if(comP.header.type == TYPE_WALLS) {
                int cnt = 0, start = 0, stop = 0, color = 0;
                QPixmap px(ui->wallOutput->width(),ui->wallOutput->height());
                px.fill(Qt::black);
                QPainter p(&px);

                //paint wall in ui
                for(int i = 0; i<comP.header.size/4; i++) {
					int val = *(((int*)comP.data) + i);
					qDebug() << "Val " << i << ": " << val;

                    if(cnt == 0)
                        start = val;
                    else if(cnt == 1)
                        stop = val;
                    else if(cnt == 2)
                        color = val;
                    cnt++;
                    if(cnt>2){ //wall part complete, draw on ui
                        cnt = 0;
                        if(color == 1){
                            p.setBackgroundMode(Qt::OpaqueMode);
                            p.setBackground(Qt::black);
                            QPen pen(Qt::black, 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);

                            p.setPen(pen);
                            p.setBrush(QBrush(Qt::black,Qt::BDiagPattern));
                        }
                        else{
                            p.setBackgroundMode(Qt::OpaqueMode);
                            p.setBackground(Qt::white);
                            QPen pen(Qt::white, 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);

                            p.setPen(pen);
                            p.setBrush(QBrush(Qt::white,Qt::BDiagPattern));
                        }

                        int winstart = ((ui->wallOutput->width()*start)/1280);
                        int winstop = ((ui->wallOutput->width()*stop)/1280);

                        QRect *rect = new QRect(winstart,1,winstop-winstart,ui->wallOutput->height()-2);
                        p.drawRect(*rect);
                    }
				}
                QPen pen(Qt::yellow, 5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
                p.setPen(pen);
                p.drawLine((ui->wallOutput->width()/2),0,ui->wallOutput->width()/2,ui->wallOutput->height());

                ui->wallOutput->clear();
                ui->wallOutput->setPixmap(px);
            }
            else if(comP.header.type == 2 && TYPE_TWO){
				for(int i = 0; i<comP.header.size/4; i++) {
					int val = *(((int*)comP.data) + i);
					char str[256];
					snprintf(str,255,"%x",val);
					qDebug() << "Val " << i << ": " << str;
				}
            }
            else if(comP.header.type == TYPE_IMAGE) {
				int imgSize = comP.header.size/2;
				char* RGBData = (char*)malloc(imgSize*3);
				for(int i = 0; i<imgSize*2; i++) {
					short* YUV = (short*)(comP.data) + i;
					RGBData[i*3+0] = (*YUV) >> 8;
					RGBData[i*3+1] = (*YUV) >> 8;
					RGBData[i*3+2] = (*YUV) >> 8;
				}
				//QImage i;
				//i = QImage(RGBData,width,height,QImage::Format_RGB888);
				QImage i((uchar*)RGBData,160,120,QImage::Format_RGB888);
				ui->imgOutput->setPixmap(QPixmap::fromImage(i));
			}
            else if(comP.header.type == TYPE_OBJECT_FOUND) {
                int dataSize = comP.header.size/2;

                uint16_t *pDataSrc = (uint16_t*)comP.data;

                if(dataSize > 6)
                    return;

                uint16_t array[10];
                for(int i = 0; i<dataSize; i++)
                {
                    array[i] = *pDataSrc++;
                }

                QPixmap px(ui->imgOutput->width(),ui->imgOutput->height());
                px.fill(QColor(0,0,0));

                int xstart = array[1];
                int xstop = array[2];
                int ystart = array[3];
                int ystop = array[4];

                int xloc = ((ui->imgOutput->width()*xstart)/1280);

                drawObjectOnPixmap(px, QPoint(xloc,ui->imgOutput->height()/2), (enum eOBJECTS)array[5], (enum eCOLOR)array[0]);

                QPainter p(&px);
                QPen pen(Qt::yellow, 5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
                p.setPen(pen);
                p.drawLine((ui->imgOutput->width()/2),0,ui->imgOutput->width()/2,ui->imgOutput->height());

                ui->imgOutput->setPixmap(px);

                ui->lblXc->setNum((xstop-xstart) + xstart);
                ui->lblYc->setNum((ystop-ystart)+ ystart);
                ui->lblXstart->setNum(xstart);
                ui->lblXstop->setNum(xstop);
                ui->lblYstart->setNum(ystart);
                ui->lblYstop->setNum(ystop);

			} else if(comP.header.type == TYPE_MAP) {
				struct wall {
					float angle,dist;
					float x1,y1;
					float x2,y2;
				};

                int wallCount = comP.header.size / sizeof(struct wall);
				struct wall* pW = (struct wall*)comP.data;

				float minx = 0, maxx = 0, miny = 0, maxy = 0;
				for(int i = 0; i<wallCount; i++) {
					if(pW[i].x1 < minx) minx = pW[i].x1;
					if(pW[i].x2 < minx) minx = pW[i].x2;
					if(pW[i].x1 > maxx) maxx = pW[i].x1;
					if(pW[i].x2 > maxx) maxx = pW[i].x2;
					if(pW[i].y1 < miny) miny = pW[i].y1;
					if(pW[i].y2 < miny) miny = pW[i].y2;
					if(pW[i].y1 > maxy) maxy = pW[i].y1;
					if(pW[i].y2 > maxy) maxy = pW[i].y2;
				}
				// stretch the render view slightly for nice results.
				minx -= 1.f;
				maxx += 1.f;
				miny -= 1.f;
				maxy += 1.f;

				float width = maxx - minx;
				float height = maxy - miny;
				float scaleWidth = ui->drawOutput->width();
				float scaleHeight = ui->drawOutput->height();

				QPixmap px(ui->drawOutput->width(),ui->drawOutput->height());
				QPainter p(&px);

                p.setPen(Qt::white);
                int ypos = ui->drawOutput->height()/2;
                p.drawPie(20,ypos-15,30,30,0,360*16);
                p.drawLine(45,ypos,60,ypos);

                p.setPen(Qt::yellow);
				for(int i = 0; i<wallCount; i++) {
					p.drawLine(
								((pW[i].x1 - minx) / width * scaleWidth),
                                (((pW[i].y1 - miny) / height * scaleHeight)),
								((pW[i].x2 - minx) / width * scaleWidth),
                                (((pW[i].y2 - miny) / height * scaleHeight))
					);

                    //p.setPen(Qt::white);
                    //QRect textrect(QPoint((int)pW[i].x1,(int)pW[i].y1),QSize(80,500));
                    //p.drawText(textrect, QString("(%1,%2)").arg(pW[i].x1).arg(pW[i].y1));

                    qDebug() << "Wall[" << i << "]: a" << pW[i].angle << ", d" << pW[i].dist << ". Pos:{" << pW[i].x1 << "," << pW[i].y1 << "},{" << pW[i].x2 << "," << pW[i].y2 << "}.";
				}

				p.end();
                ui->drawOutput->setPixmap(px);
			}
            //writePacket(comP.header.type+1,comP.header.size,comP.data);
        }
        else if(comP.header.type >= TYPE_MAX) {
            //type nummer to high indicates a communication failure, send a comreset packet to the robot.
            //the robot will wait a while when the application resets his comReadPos

            qDebug() << "Error type. " << _FILE_LINE_;
            qDebug() << "Request comreset from robot";
            char data[] = {0x55, 0xAA, 0x00};
            writePacket(TYPE_RESET_COM,2,(void*)data);

            Sleep(100);
            comReadPos = 0;
        }
        else if(ret == -1) {
			qDebug() << "Error reading packet." << _FILE_LINE_;
        }
        else {
            //qDebug() << "Read nothing.";
		}
        comTimer->start(100); //change speed
	}
    else
        comTimer->start(1000); //change speed
}

void drawTest(QPixmap &px) {
	QPainter p(&px);

	// draw a bunch of wibbly wobbly lines
	QPen pen(QColor(0,0,255));
	p.setPen(pen);
	p.drawLine(10,10,10,px.height()-10); // blue line from top left to bottom left
	pen.setColor(QColor(0,255,0));
	p.setPen(pen);
	p.drawLine(10,10,px.width()-10,10); // green line from top left to top right
	pen.setColor(QColor(255,0,0));
	p.setPen(pen);
	p.drawLine(10,10,px.width()-10,px.height()-10); // red line from top left to bottom right
}

void drawObjectOnPixmap(QPixmap &px, QPoint loc, enum eOBJECTS obj ,enum eCOLOR color){
    QPainter p(&px);
    QString sclr, sobj;

    if(color < COLOR_SIZE && obj < OBJ_SIZE){
        if(color == COLOR_GREEN){
            p.setPen(Qt::green);
            sclr = "green";
        }
        else if(color == COLOR_RED){
            p.setPen(Qt::red);
            sclr = "red";
        }
        else if(color == COLOR_BLUE){
            p.setPen(Qt::blue);
            sclr = "blue";
        }

        if(obj == OBJ_UNKNOWN){
            p.drawLine(loc + QPoint(-10,-10),loc + QPoint(+10,+10));
            p.drawLine(loc + QPoint(+10,-10),loc + QPoint(-10,+10));
            sobj = "unknow";
            loc += QPoint(-10,-10);
        }
        else if(obj == OBJ_SQUARE){
            loc += QPoint(-10,-10);
            QRect rect(loc,QSize(20,20));
            p.drawRect(rect);
            sobj = "square";
        }
        else if(obj == OBJ_TRIANGLE){
            QPoint top = loc - QPoint(0,10);
            p.drawLine(top, (top + QPoint(-10,20)));
            p.drawLine(top, (top + QPoint(10,20)));
            p.drawLine((top + QPoint(10,20)), (top + QPoint(-10,20)));
            sobj = "triangle";
            loc += QPoint(-10,-10);
        }
        else if(obj == OBJ_BAR){
            loc += QPoint(-5,-10);
            QRect rect(loc,QSize(10,20));
            p.drawRect(rect);
            sobj = "bar";
        }
        else{

        }

        QRect textrect(loc + QPoint(30,0),QSize(80,200));
        p.setPen(Qt::white);

        p.drawText(textrect, QString(" Obj: %1\n C:    %2").arg(sobj).arg(sclr));
        p.setPen(Qt::black);
        p.drawRect(textrect);
    }
}

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    hCom = NULL;
    ui->setupUi(this);

    setWindowTitle("AmaseApp");

    comTimer = new QTimer(this);
    connect(comTimer,SIGNAL(timeout()),this,SLOT(update()));
    comTimer->start(10);

    QPixmap px1(ui->drawOutput->width(),ui->drawOutput->height());
    px1.fill(QColor(0,0,0));
    ui->drawOutput->setPixmap(px1);

    QPixmap px(ui->imgOutput->width(),ui->imgOutput->height());
    px.fill(QColor(0,0,0));

    drawObjectOnPixmap(px, QPoint(15,15), OBJ_SQUARE, COLOR_RED);
    drawObjectOnPixmap(px, QPoint(105,15), OBJ_BAR, COLOR_GREEN);
    drawObjectOnPixmap(px, QPoint(205,15), OBJ_TRIANGLE, COLOR_BLUE);
    ui->imgOutput->setPixmap(px);

    QPixmap px2(ui->wallOutput->width(),ui->wallOutput->height());
    px2.fill(QColor(0,0,0));
    ui->wallOutput->setPixmap(px2);
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::on_COMconnect_clicked()
{
	if(hCom == NULL) {
		QString s = ui->COMcomboBox->currentText();
        if(s.count() > 0) {
            int index = s.toInt();
            qDebug() << "Connect to com:" << index;
			int status = openCom(index);
			if(status == 0) {
				ui->COMconnect->setText(QString("Disconnect"));
			} else {
				qDebug() << "Could not open COM port " << index << "." << _FILE_LINE_;
			}
			ui->COMcomboBox->clear();
        }
        if(status == 0) {
            ui->COMconnect->setText(QString("Disconnect"));
        } else {
            qDebug() << "Could not open COM port " << 24 << "." << _FILE_LINE_;
        }
	} else {
		closeCom();
		ui->COMconnect->setText(QString("Connect"));
		update();
	}
}

void MainWindow::on_COMrefresh_clicked()
{
    int ports = fillComPortList(0,30);
    ui->COMcomboBox->clear();
    for(int i = 0; i<ports; i++) {
        QString s = QString::number(comPortList[i]);
        ui->COMcomboBox->addItem(s,s);
    }
}

void MainWindow::on_pbSetFrontLed_clicked()
{
    static char ledSet = 0;

    if(hCom != NULL) {
        if(ledSet == 0){
            ui->pbSetFrontLed->setText("FrontLED = ON");
            ledSet = 1;
        }
        else{
            ui->pbSetFrontLed->setText("FrontLED = OFF");
            ledSet = 0;
        }
        writePacket(TYPE_FRONTLED_SET, 1,(void *) &ledSet);
    }
}
